#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static void _my_moments(const cv::Mat& src, std::vector<double>& spatialMoments, 
                        std::vector<double>& centralMoments, std::vector<double>& normalizedCentralMoments,
                        bool binaryImage = false) {
    spatialMoments.clear();
    centralMoments.clear();
    normalizedCentralMoments.clear();
    double m00 = 0, m10 = 0, m01 = 0, m20 = 0, m11 = 0, m02 = 0, m30 = 0, m21 = 0, m12 = 0, m03 = 0;
    double mu00 = 0, mu10 = 0, mu01 = 0, mu20 = 0, mu11 = 0, mu02 = 0, mu30 = 0, mu21 = 0, mu12 = 0, mu03 = 0;
    double nu00 = 0, nu10 = 0, nu01 = 0, nu20 = 0, nu11 = 0, nu02 = 0, nu30 = 0, nu21 = 0, nu12 = 0, nu03 = 0;
    for (int x = 0; x < src.cols; x++) {
        for (int y = 0; y < src.rows; y++) {
            double Ixy = src.ptr<uchar>(y)[x];
            if (binaryImage && Ixy > 0) {
                Ixy = 1;
            }
            m00 += Ixy;
            m10 += x * Ixy;
            m01 += y * Ixy;
            m20 += x * x * Ixy;
            m11 += x * y * Ixy;
            m02 += y * y * Ixy;
            m30 += x * x * x * Ixy;
            m21 += x * x * y * Ixy;
            m12 += x * y * y * Ixy;
            m03 += y * y * y * Ixy;
        }
    }
    
    double mean_x = m10 / m00;
    double mean_y = m01 / m00;
    mu00 = m00;
    for (int x = 0; x < src.cols; x++) {
        for (int y = 0; y < src.rows; y++) {
            double Ixy = src.ptr<uchar>(y)[x];
            if (binaryImage && Ixy > 0) {
                Ixy = 1;
            }
            double xm = x - mean_x;
            double ym = y - mean_y;
            mu20 += xm * xm * Ixy;
            mu11 += xm * ym * Ixy;
            mu02 += ym * ym * Ixy;
            mu30 += xm * xm * xm * Ixy;
            mu21 += xm * xm * ym * Ixy;
            mu12 += xm * ym * ym * Ixy;
            mu03 += ym * ym * ym * Ixy;
        }
    }
    
    nu00 = 1;
    nu10 = nu01 = 0;
    nu20 = mu20 / std::pow(m00, (2+0)/2.+1);
    nu11 = mu11 / std::pow(m00, (1+1)/2.+1);
    nu02 = mu02 / std::pow(m00, (0+2)/2.+1);
    nu30 = mu30 / std::pow(m00, (3+0)/2.+1);
    nu21 = mu21 / std::pow(m00, (2+1)/2.+1);
    nu12 = mu12 / std::pow(m00, (1+2)/2.+1);
    nu03 = mu03 / std::pow(m00, (0+3)/2.+1);
    
    spatialMoments.push_back(m00); spatialMoments.push_back(m10);
    spatialMoments.push_back(m01); spatialMoments.push_back(m20);
    spatialMoments.push_back(m11); spatialMoments.push_back(m02);
    spatialMoments.push_back(m30); spatialMoments.push_back(m21);
    spatialMoments.push_back(m12); spatialMoments.push_back(m03);
    
    centralMoments.push_back(mu00); centralMoments.push_back(mu10);
    centralMoments.push_back(mu01); centralMoments.push_back(mu20);
    centralMoments.push_back(mu11); centralMoments.push_back(mu02);
    centralMoments.push_back(mu30); centralMoments.push_back(mu21);
    centralMoments.push_back(mu12); centralMoments.push_back(mu03);
    
    normalizedCentralMoments.push_back(nu00); normalizedCentralMoments.push_back(nu10);
    normalizedCentralMoments.push_back(nu01); normalizedCentralMoments.push_back(nu20);
    normalizedCentralMoments.push_back(nu11); normalizedCentralMoments.push_back(nu02);
    normalizedCentralMoments.push_back(nu30); normalizedCentralMoments.push_back(nu21);
    normalizedCentralMoments.push_back(nu12); normalizedCentralMoments.push_back(nu03);
}

static void _hu_invariant_moments(const cv::Moments& moments, double HuMoments[]) {
    double nu00 = 1, nu10 = 0, nu01 = 0;
    double nu20 = moments.nu20;
    double nu11 = moments.nu11;
    double nu02 = moments.nu02;
    double nu30 = moments.nu30;
    double nu21 = moments.nu21;
    double nu12 = moments.nu12;
    double nu03 = moments.nu03;
    
    HuMoments[0] = nu20 + nu02;
    HuMoments[1] = std::pow(nu20-nu02, 2) + 4*std::pow(nu11, 2);
    HuMoments[2] = std::pow(nu30-3*nu12, 2) + std::pow(3*nu21-nu03, 2);
    HuMoments[3] = std::pow(nu30+nu12, 2) + std::pow(nu21+nu03, 2);
    HuMoments[4] = (nu30-3*nu12)*(nu30+nu12)*(std::pow(nu30+nu12, 2)-3*std::pow(nu21+nu03, 2)) + 
                (3*nu21-nu03)*(nu21+nu03)*(3*std::pow(nu30+nu12, 2)-std::pow(nu21+nu03, 2));
    HuMoments[5] = (nu20-nu02)*(std::pow(nu30+nu12, 2)-std::pow(nu21+nu03, 2)) + 4*nu11*(nu30+nu12)*(nu21+nu03);
    HuMoments[6] = (3*nu21-nu03)*(nu30+nu12)*(std::pow(nu30+nu12, 2)-3*std::pow(nu21+nu03, 2)) - 
                (nu30-3*nu12)*(nu21+nu03)*(3*std::pow(nu30+nu12, 2)-std::pow(nu21+nu03, 2));
}

int main(int argc, char **argv) {
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_GRAYSCALE);
    if (src.empty()) {
        std::cout << "failed to read image" << std::endl;
        return EXIT_FAILURE;
    }
    
    /*bool binaryImage = true;
    std::vector<cv::String> spatialMomentLabels = 
        {"m00 = ", "m10 = ", "m01 = ", "m20 = ", "m11 = ", "m02 = ", "m30 = ", "m21 = ", "m12 = ", "m03 = "};
    std::vector<cv::String> centralMomentLabels = 
        {"mu00 = ", "mu10 = ", "mu01 = ", "mu20 = ", "mu11 = ", "mu02 = ", "mu30 = ", "mu21 = ", "mu12 = ", "mu03 = "};
    std::vector<cv::String> normalizedCentralMomentLabels = 
        {"nu00 = ", "nu10 = ", "nu01 = ", "nu20 = ", "nu11 = ", "nu02 = ", "nu30 = ", "nu21 = ", "nu12 = ", "nu03 = "};
    
    cv::Moments m = cv::moments(src, binaryImage);
    
    // spatial moments
    std::cout << "========spatial moments=============" << std::endl;
    std::cout << spatialMomentLabels[0] << m.m00 << std::endl;
    std::cout << spatialMomentLabels[1] << m.m10 << std::endl;
    std::cout << spatialMomentLabels[2] << m.m01 << std::endl;
    std::cout << spatialMomentLabels[3] << m.m20 << std::endl;
    std::cout << spatialMomentLabels[4] << m.m11 << std::endl;
    std::cout << spatialMomentLabels[5] << m.m02 << std::endl;
    std::cout << spatialMomentLabels[6] << m.m30 << std::endl;
    std::cout << spatialMomentLabels[7] << m.m21 << std::endl;
    std::cout << spatialMomentLabels[8] << m.m12 << std::endl;
    std::cout << spatialMomentLabels[9] << m.m03 << std::endl;
    std::vector<double> spatialMoments, centralMoments, normalizedCentralMoments;
    _my_moments(src, spatialMoments, centralMoments, normalizedCentralMoments, binaryImage);
    std::cout << "    -------------    " << std::endl;
    for (int i = 0; i < spatialMoments.size(); i++) {
        std::cout << spatialMomentLabels[i] << spatialMoments[i] << std::endl;
    }
    
    // central moments
    std::cout << "========central moments=============" << std::endl;
    std::cout << centralMomentLabels[0] << m.m00 << std::endl;
    std::cout << centralMomentLabels[1] << 0 << std::endl;
    std::cout << centralMomentLabels[2] << 0 << std::endl;
    std::cout << centralMomentLabels[3] << m.mu20 << std::endl;
    std::cout << centralMomentLabels[4] << m.mu11 << std::endl;
    std::cout << centralMomentLabels[5] << m.mu02 << std::endl;
    std::cout << centralMomentLabels[6] << m.mu30 << std::endl;
    std::cout << centralMomentLabels[7] << m.mu21 << std::endl;
    std::cout << centralMomentLabels[8] << m.mu12 << std::endl;
    std::cout << centralMomentLabels[9] << m.mu03 << std::endl;
    std::cout << "    -------------    " << std::endl;
    for (int i = 0; i < centralMoments.size(); i++) {
        std::cout << centralMomentLabels[i] << centralMoments[i] << std::endl;
    }
    
    // normalized central moments
    // central moments
    std::cout << "========normalized central moments=============" << std::endl;
    std::cout << normalizedCentralMomentLabels[0] << 1 << std::endl;
    std::cout << normalizedCentralMomentLabels[1] << 0 << std::endl;
    std::cout << normalizedCentralMomentLabels[2] << 0 << std::endl;
    std::cout << normalizedCentralMomentLabels[3] << m.nu20 << std::endl;
    std::cout << normalizedCentralMomentLabels[4] << m.nu11 << std::endl;
    std::cout << normalizedCentralMomentLabels[5] << m.nu02 << std::endl;
    std::cout << normalizedCentralMomentLabels[6] << m.nu30 << std::endl;
    std::cout << normalizedCentralMomentLabels[7] << m.nu21 << std::endl;
    std::cout << normalizedCentralMomentLabels[8] << m.nu12 << std::endl;
    std::cout << normalizedCentralMomentLabels[9] << m.nu03 << std::endl;
    std::cout << "    -------------    " << std::endl;
    for (int i = 0; i < normalizedCentralMoments.size(); i++) {
        std::cout << normalizedCentralMomentLabels[i] << normalizedCentralMoments[i] << std::endl;
    }*/
    
    double hu[7], HuMoments[7];
    cv::Moments m = cv::moments(src, false);
    cv::HuMoments(m, hu);
    _hu_invariant_moments(m, HuMoments);
    for (int i = 0; i < 7; i++) {
        if (std::abs(hu[i] - HuMoments[i]) > DBL_EPSILON) {
            std::cout << "Hu Moments " << (i+1) << " are not same!" << std::endl;
            std::cout << "\tOpenCV's = " << hu[i] << std::endl;
            std::cout << "\tNot OpenCV's = " << HuMoments[i] << std::endl;
        } else {
            std::cout << "Hu Moments " << (i+1) << " are the same!" << std::endl;
            std::cout << "\tHu Moments " << (i+1) << " = " << hu[i] << std::endl;
        }
    }
    
    return 0;
}
